#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <pcl/io/ply_io.h>


#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>


#include <iostream>
#include <sstream>
#include <string>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

class PointCloudFilter {
public:
    PointCloudFilter(ros::NodeHandle& nh, std::string file_path) : nh_(nh), path(file_path){
        sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, &PointCloudFilter::cloudCallback, this);
        sub1_ = nh_.subscribe<sensor_msgs::Image>("/camera/color/image_raw", 1, &PointCloudFilter::imageCallback, this);
        save_counter_ = 0;
        std::cout << "path:" << path <<std::endl;
    }

    std::string PointCloudSave() {
        if (!cloud || cloud->empty()) {
            ROS_WARN("PointCloud is not initialized or empty.");
            return "";
        }

        std::string filename = path + "/pointcloud_" + std::to_string(save_counter_++) + ".ply";
        pcl::io::savePLYFile(filename, *cloud);
        ROS_INFO("Saved filtered point cloud to %s", filename.c_str());
        return filename;
    }

    std::string saveImage() {
        if (!cv_ptr) {
            ROS_WARN("Image is not initialized.");
            return "";
        }

        cv::Mat rgb_image;
        cv::cvtColor(cv_ptr->image, rgb_image, cv::COLOR_BGR2RGB);

        std::string filename = path + "/rgb_" + std::to_string(save_counter_ - 1) + ".png";
        cv::imwrite(filename, rgb_image);
        ROS_INFO("Saved image: %s", filename.c_str());
        return filename;
    }

private:
    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input) {
        cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(*input, *cloud);
    }

    void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
        if (msg->encoding == "rgb8") {
            cv_ptr = cv_bridge::toCvCopy(msg, "rgb8");
        } else if (msg->encoding == "bgr8") {
            cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
        }
    }

    ros::NodeHandle nh_;
    ros::Subscriber sub_, sub1_;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
    cv_bridge::CvImagePtr cv_ptr;
    int save_counter_;
    std::string path;
};

// HTTP 服务器类
class WebServer {
public:
    WebServer(PointCloudFilter* filter, int port) //接收"1个类"和"端口号"
        : io_service_(), acceptor_(io_service_, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port)), //监听本地的端口号
          socket_(io_service_), filter_(filter) {
        start_accept();
        io_service_.run(); //启动事件循环，持续处理网络事件
    }

private:
    void start_accept() { //启动异步TCP连接监听，等待客户端请求,当有TCP请求时，绑定回调函数：handle_accept
        acceptor_.async_accept(socket_,
            boost::bind(&WebServer::handle_accept, this,  
                        boost::asio::placeholders::error)); 
    }

    void handle_accept(const boost::system::error_code& error) {
        if (!error) {
            boost::asio::async_read_until(
                socket_, response_, "\r\n\r\n",
                boost::bind(&WebServer::handle_read, this,
                            boost::asio::placeholders::error,
                            boost::asio::placeholders::bytes_transferred));
        } else {
            std::cerr << "Accept error: " << error.message() << std::endl;
            start_accept();
        }
    }

    void handle_read(const boost::system::error_code& error, size_t bytes_transferred) {
        if (!error) {
            std::string request_str = boost::asio::buffer_cast<const char*>(response_.data());
            request_str = request_str.substr(0, bytes_transferred);
            response_.consume(bytes_transferred);

            if (boost::starts_with(request_str, "GET /get_pointcloud_image")) {
                
                std::string response;
                std::string pc_file, img_file;
                std::ostringstream oss;

                pc_file = filter_->PointCloudSave();
                img_file = filter_->saveImage();

                oss << R"({"point_cloud_file":")" << pc_file << R"(", "image_file":")" << img_file << R"("})";
                
                response =  "HTTP/1.1 200 OK\r\n"
                            "Content-Type: application/json\r\n"
                            "Content-Length: " + std::to_string(oss.str().size()) + "\r\n"
                            "Connection: close\r\n"
                            "\r\n" + oss.str();

                boost::asio::async_write(
                    socket_,
                    boost::asio::buffer(response),
                    [this](const boost::system::error_code&, size_t) {
                        socket_.close();
                        start_accept();
                });
              
            } else {
                std::string response = "HTTP/1.1 404 Not Found\r\n\r\n";
                boost::asio::async_write(
                    socket_,
                    boost::asio::buffer(response),
                    [this](const boost::system::error_code&, size_t) {
                        socket_.close();
                        start_accept();
                });
            }
        }
        else {
            std::cerr << "Read error: " << error.message() << std::endl;
            socket_.close();
            start_accept();
        }
    }

    boost::asio::io_service io_service_;
    boost::asio::ip::tcp::acceptor acceptor_;
    boost::asio::ip::tcp::socket socket_;
    boost::asio::streambuf response_;
    PointCloudFilter* filter_;
};


int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_filter_node");
    ros::NodeHandle nh("~");

    std::string file_path;
    int port;
    

    nh.param<std::string>("file_path", file_path, "/home/c/ros_ws");
    nh.param<int>("port", port, 8080);
  

    ROS_INFO("file_path : %s, port : %d", file_path.c_str(), port);
 

    // PointCloudFilter filter(nh, file_path);
    PointCloudFilter* filter = new PointCloudFilter(nh, file_path); // 堆上创建
    WebServer web_server(filter, port);
    
    ros::spin();  // 保持程序运行，处理回调和事件

    delete filter; // 手动释放内存，避免内存泄漏
    return 0;
}